//
// Created by 31853 on 2024/6/13.
//

#include "UPRE_Task_Flow.h"

#include "fdcan.h"
#include "slowStart.h"
#include "Task scheduling_l.h"
#include "UPRE_DIJKSTRA.h"
#include "UPRE_LOCATION.h"

Task_flag TaskFlag = {0};

Handshake hand_shake = {0};

extern uint8_t Ball[12];

extern int graph[V][V];

static void PathNumToFlag(void) {
    if (Current_PathNum >= 0 && Current_PathNum <= 5) {
        TaskFlag.Flag_GetFrontBall = true;
    } else if (Current_PathNum >= 6 && Current_PathNum <= 11) {
        TaskFlag.Flag_GetRearBall = true;
    } else if (Current_PathNum == 21) {
        if (TaskFlag.Flag_ExchangePath) {
            TaskFlag.Flag_second_PutSeding_R = true;
        } else {
            TaskFlag.Flag_third_PutSeding_R = true;
        }
    } else if (Current_PathNum == 23) {
        if (TaskFlag.Flag_ExchangePath) {
            TaskFlag.Flag_second_PutSeding_L = true;
        } else {
            TaskFlag.Flag_third_PutSeding_L = true;
        }
    } else if (Current_PathNum == 24) {
        if (TaskFlag.Flag_ExchangePath) {
            TaskFlag.Flag_first_PutSeding_L = true;
        } else {
            TaskFlag.Flag_second_PutSeding_L = true;
        }
    } else if (Current_PathNum == 26) {
        if (TaskFlag.Flag_ExchangePath) {
            TaskFlag.Flag_first_PutSeding_R = true;
        } else {
            TaskFlag.Flag_second_PutSeding_R = true;
        }
    } else if (Current_PathNum == 27) {
        TaskFlag.Flag_first_PutSeding_R = true;
    } else if (Current_PathNum == 29) {
        TaskFlag.Flag_first_PutSeding_L = true;
    } else if (Current_PathNum == 30) {
        TaskFlag.Flag_third_GetSeding = true;
    } else if (Current_PathNum == 31) {
        if (TaskFlag.Flag_ExchangePath) {
            TaskFlag.Flag_third_GetSeding = true;
        } else {
            TaskFlag.Flag_second_GetSeding = true;
        }
    } else if (Current_PathNum == 32) {
        if (TaskFlag.Flag_ExchangePath) {
            TaskFlag.Flag_second_GetSeding = true;
        } else {
            TaskFlag.Flag_first_GetSeding = true;
        }
    } else {
    }
    Current_PathNum = 255;
}


extern uint8_t Loop_Num;
extern uint8_t Knots_Num;

float key_yaw = 0.f;

extern uint8_t current_knot;

void TaskExecution(void) {
    static bool onceGetColor = false;
    PathNumToFlag();
    if (!TaskFlag.Flag_OnceParking) {
        if (TaskFlag.Flag_StartParking) {
            /*******Start Parking Begin*********/
            TaskFlag.Flag_ManualMode = true;
            /*******Start Parking End*********/
            ///TODO::::::摆车完成后，提供一个标志位，便于清空和取消执行
            TaskFlag.Flag_StopParking = true;
            TaskFlag.Flag_StartParking = false;
        }
    }


    if (TaskFlag.Flag_Left90) {
        hand_shake.Handshake_Yaw.yaw -= 90.f;
        // printf("Flag_Left90\r\n");
        key_yaw += 90.f;
        // osDelay(5);
        TaskFlag.Flag_Left90 = false;
    }

    if (TaskFlag.Flag_Right90) {
        // printf("Flag_Right90\r\n");
        // hand_shake.Handshake_Yaw.yaw += 90.f;
        key_yaw -= 90.f;
        // osDelay(5);
        TaskFlag.Flag_Right90 = false;
    }

    if (TaskFlag.Flag_first_GetSeding) {
        /*********GetSeding First Begin*********/
        // printf("Flag_first_GetSeding\r\n");
        Main_Control.task_state = claw_grain;
        // osDelay(5);
        /*********GetSeding First End*********/
        TaskFlag.Flag_first_GetSeding = false;
    }

    if (TaskFlag.Flag_second_GetSeding) {
        /*********GetSeding Second Begin*********/
        // printf("Flag_second_GetSeding\r\n");
        Main_Control.task_state = claw_grain;
        // osDelay(5);
        /*********GetSeding Second End*********/
        TaskFlag.Flag_second_GetSeding = false;
    }

    if (TaskFlag.Flag_third_GetSeding) {
        /*********GetSeding Third Begin*********/
        // printf("Flag_third_GetSeding\r\n");
        Main_Control.task_state = claw_grain;
        // osDelay(5);
        /*********GetSeding Third End*********/
        TaskFlag.Flag_third_GetSeding = false;
    }

    if (TaskFlag.Flag_first_PutSeding_L) {
        /*********PutSeding First Left Begin*********/
        // printf("Flag_first_PutSeding_L\r\n");
        Main_Control.task_state = lay_grain;
        // osDelay(5);
        /*********PutSeding First Left End*********/
        TaskFlag.Flag_first_PutSeding_L = false;
    }

    if (TaskFlag.Flag_first_PutSeding_R) {
        /*********PutSeding First Right Begin*********/
        // printf("Flag_first_PutSeding_R\r\n");
        Main_Control.task_state = lay_grain;
        // osDelay(5);
        /*********PutSeding First Right End*********/
        TaskFlag.Flag_first_PutSeding_R = false;
    }

    if (TaskFlag.Flag_second_PutSeding_L) {
        /*********PutSeding Second Left Begin*********/
        // printf("Flag_second_PutSeding_L\r\n");
        Main_Control.task_state = lay_grain;
        // osDelay(5);
        /*********PutSeding Second Left End*********/
        TaskFlag.Flag_second_PutSeding_L = false;
    }

    if (TaskFlag.Flag_second_PutSeding_R) {
        /*********PutSeding Second Left Begin*********/
        // printf("Flag_second_PutSeding_R\r\n");
        Main_Control.task_state = lay_grain;
        // osDelay(5);
        /*********PutSeding Second Left End*********/
        TaskFlag.Flag_second_PutSeding_R = false;
    }

    if (TaskFlag.Flag_third_PutSeding_L) {
        /*********PutSeding Third Right Begin*********/
        // printf("Flag_third_PutSeding_L\r\n");
        Main_Control.task_state = lay_grain;
        // osDelay(5);
        /*********PutSeding Third Right End*********/
        TaskFlag.Flag_third_PutSeding_L = false;
    }

    if (TaskFlag.Flag_third_PutSeding_R) {
        /*********PutSeding Third Right Begin*********/
        // printf("Flag_third_PutSeding_R\r\n");
        Main_Control.task_state = lay_grain;
        // osDelay(5);
        /*********PutSeding Third Right End*********/
        TaskFlag.Flag_third_PutSeding_R = false;
    }

    if (TaskFlag.Flag_GetFrontBall) {
        /*********Get Front Ball Begin*********/
        // printf("Flag_GetFrontBall\r\n");
        Main_Control.task_state = front_receball;
        // osDelay(5);
        /*********Get Front Ball End*********/
        TaskFlag.Flag_GetFrontBall = false;
    }

    if (TaskFlag.Flag_GetRearBall) {
        /*********Get Rear Ball Begin*********/
        // printf("Flag_GetRearBall\r\n");
        Main_Control.task_state = belly_receball;
        // osDelay(5);
        /*********Get Rear Ball End*********/
        TaskFlag.Flag_GetRearBall = false;
    }

    if (TaskFlag.Flag_ShootBall) {
        /*********Shoot Ball Begin*********/
        // printf("Flag_ShootBall\r\n");
        Main_Control.task_state = shoot_ball;
        // osDelay(5);
        /*********Shoot Ball End*********/
        TaskFlag.Flag_ShootBall = false;
    }

    if (TaskFlag.Flag_AddShootBallSpeed) {
        /*********Add Shoot Ball Speed Begin*********/
        // printf("Flag_AddShootBallSpeed\r\n");
        Main_Control.task_state = accelerate_shoot;
        // osDelay(5);
        /*********Add Shoot Ball Speed End*********/
        TaskFlag.Flag_AddShootBallSpeed = false;
    }

    if (TaskFlag.Flag_SubtractShootBallSpeed) {
        /*********Subtract Shoot Ball Speed Begin*********/
        // printf("Flag_SubtractShootBallSpeed\r\n");
        Main_Control.task_state = decelerate_shoot;
        // osDelay(5);
        /*********Subtract Shoot Ball Speed End*********/
        TaskFlag.Flag_SubtractShootBallSpeed = false;
    }

    if (TaskFlag.Flag_ResetShootBallSpeed) {
        /*********Reset Shoot Ball Speed Begin*********/
        // printf("Flag_ResetShootBallSpeed\r\n");
        // osDelay(5);
        ////TODO
        /*********Reset Shoot Ball Speed End*********/
        TaskFlag.Flag_ResetShootBallSpeed = false;
    }

    if (TaskFlag.Flag_ExchangeManualMode) {
        /*********Exchange Manual Mode Begin*********/
        if (TaskFlag.Flag_ManualMode) {
            // printf("No Flag_ManualMode\r\n");
            TaskFlag.Flag_ManualMode = false;
        } else {
            // printf("Yes Flag_ManualMode\r\n");
            TaskFlag.Flag_ManualMode = true;
        }
        // osDelay(5);
        /*********Exchange Manual Mode End*********/
        TaskFlag.Flag_ExchangeManualMode = false;
    }
    ///开始前按
    extern uint8_t bit_array[32];

    if (!TaskFlag.Flag_Start) {
        if (TaskFlag.Flag_ExchangePath) {
            ///TODO:::这里谁都不要动,动了程序会直接进死循环
            graph[20][33] = 0;
            graph[33][20] = 0;

            graph[26][31] = 1;
            graph[31][26] = 1;
            ///
            Dijkstra_FirstPathPlanning(33);
            // osDelay(5);
            TaskFlag.Flag_ExchangePath = false;
        }
    }

    if (!onceGetColor) {
        if (TaskFlag.Flag_GetBallColcor) {
            /*********GetSeding First Begin*********/
            for (int i = 0; i < 12; ++i) {
                ball[i / 6][i % 6] = Ball[i];
            }
            if (DIJKSTRA_CheckBall()) {
                /*******Colcor True Begin**********/
                // printf("Get Ball Colcor Finish\r\n");
                onceGetColor = true;
                /*******Colcor True End**********/
            } else {
                /*******Colcor False Begin**********/
                // printf("Not Get Ball Colcor\r\n");
                /*******Colcor False End**********/
            }
            /*********GetSeding First End*********/
            osDelay(5);
            TaskFlag.Flag_GetBallColcor = false;
        }
    }

    if (current_knot != 12) {
        if (TaskFlag.Flag_GoTO0) {
            // printf("Flag_GoTO0\r\n");
            Dijkstra_TwoZonePathPlanning(12);
            TaskFlag.Flag_GoTO0 = false;
        }
    } else {
        if (TaskFlag.Flag_GoTO0) {
            // printf("Flag_GoTO0\r\n");
            TaskFlag.Flag_GoTO0 = false;
        }
    }

    if (current_knot != 13) {
        if (TaskFlag.Flag_GoTO1) {
            // printf("Flag_GoTO1\r\n");
            Dijkstra_TwoZonePathPlanning(13);
            TaskFlag.Flag_GoTO1 = false;
        }
    } else {
        if (TaskFlag.Flag_GoTO1) {
            // printf("Flag_GoTO0\r\n");
            TaskFlag.Flag_GoTO1 = false;
        }
    }

    if (current_knot != 14) {
        if (TaskFlag.Flag_GoTO2) {
            // printf("Flag_GoTO2\r\n");
            Dijkstra_TwoZonePathPlanning(14);
            TaskFlag.Flag_GoTO2 = false;
        }
    } else {
        if (TaskFlag.Flag_GoTO2) {
            // printf("Flag_GoTO0\r\n");
            TaskFlag.Flag_GoTO2 = false;
        }
    }

    if (current_knot != 15) {
        if (TaskFlag.Flag_GoTO3) {
            // printf("Flag_GoTO3\r\n");
            Dijkstra_TwoZonePathPlanning(15);
            TaskFlag.Flag_GoTO3 = false;
        }
    } else {
        if (TaskFlag.Flag_GoTO3) {
            // printf("Flag_GoTO3\r\n");
            TaskFlag.Flag_GoTO3 = false;
        }
    }

    if (current_knot != 16) {
        if (TaskFlag.Flag_GoTO4) {
            // printf("Flag_GoTO4\r\n");
            Dijkstra_TwoZonePathPlanning(16);
            TaskFlag.Flag_GoTO4 = false;
        }
    } else {
        if (TaskFlag.Flag_GoTO4) {
            // printf("Flag_GoTO4\r\n");
            TaskFlag.Flag_GoTO4 = false;
        }
    }

    if (current_knot != 17) {
        if (TaskFlag.Flag_GoTO5) {
            // printf("Flag_GoTO5\r\n");
            Dijkstra_TwoZonePathPlanning(17);
            TaskFlag.Flag_GoTO5 = false;
        }
    } else {
        if (TaskFlag.Flag_GoTO5) {
            // printf("Flag_GoTO0\r\n");
            TaskFlag.Flag_GoTO5 = false;
        }
    }

    // if(!TaskFlag.Flag_Start) {
        if (TaskFlag.Flag_ReStart) {
            // printf("Flag_ReStart\r\n");
            graph[20][33] = 0;
            graph[33][20] = 0;

            graph[26][31] = 0;
            graph[31][26] = 0;
            Clean_Pose();
            Dijkstra_FirstPathPlanning(33);
            TaskFlag.Flag_ReStart = false;
        }
    // }

    // if(!TaskFlag.Flag_Start) {
        if (TaskFlag.Flag_GoTwoZone) {
            // printf("Flag_GoTwoZone\r\n");

            graph[20][33] = 1;
            graph[33][20] = 1;
            Dijkstra_FirstPathPlanning(33);
            TaskFlag.Flag_GoTwoZone = false;
        }
    // }

    // if(TaskFlag.Flag_BlueZone) {
    // printf("Flag_BlueZone\r\n");
    // TaskFlag.Flag_BlueZone = false;
    // }
    //
    // if(TaskFlag.Flag_RedZone) {
    // printf("Flag_RedZone\r\n");
    // TaskFlag.Flag_RedZone = false;
    // }

    // if (TaskFlag.Flag_ManualGetSeding) {
        // printf("Flag_ManualGetSeding\r\n");
        // TaskFlag.Flag_ManualGetSeding = false;
    // }

    // if(TaskFlag.Flag_StartITPath) {
    //     if(TaskFlag.Flag_ITPath) {
    //         Loop_Num = 0;    //         Knots_Num++;
    //         TaskFlag.Flag_ITPath = false;
    //         TaskFlag.Flag_StartITPath = false;
    //     }
    // }
}
